/* UINTUTIL.C - shared functions for UINT16 Mex files */

/* By:   S.C. Molitor (smolitor@bme.jhu.edu)
   Date: February 8, 1999 */


// inclusions

#include "mex.h"
#include "uintutil.h"


// CALCDIMSTRIDE - calculate index increment along a specified dimension

int CalcDimStride(const int	*sizeX,
				  int		dimX) {

	// declare variables

	int	result;		// storage for result
	int	i;			// generic loop counter

	// calculate index increment along DIMX

	result = 1;
	for (i = 0; i < dimX; i++) {
		result *= sizeX[i];
	}
	return(result);
}


// COORDINCREMENT - increment N-D coordinate with dimension skip

void CoordIncrement(const int	*sizeX,
					int			*coordX,
					int			ndimsX,
					int			dimskip) {

	int	i;	// generic loop counter
	
	// update lowest-D coordinate value
	// do not update skip dimension coordinate
	// check for coordinate exceeding dimension size
	
	if ((dimskip == 0) && (ndimsX > 1)) {
		coordX[1]++;
	}
	else {
		coordX[0]++;
	}
	for (i = 1; i < ndimsX; i++) {
		if (coordX[i - 1] >= sizeX[i - 1]) {
			coordX[i - 1] = 0;
			if ((dimskip == i) && (ndimsX > (i + 1))) {
				coordX[i + 1]++;
			}
			else {
				coordX[i]++;
			}
		}
	}
}


// COORDINDEX - convert N-D matrix coordinate into linear array index

int CoordIndex(const int	*sizeX,
			   const int	*coordX,
			   int			ndimsX) {

	// declare variables

	int	result;		// storage for result
	int	i;			// generic loop counter
	int	prodsize;	// product of dimension sizes

	// calculate linear index from coordinate & dimensions

	result = 0;
	prodsize = 1;
	for (i = 0; i < ndimsX; i++) {
		result += coordX[i] * prodsize;
		prodsize *= sizeX[i];
	}
	return(result);
}


// CREATELOGICAL - create logical output array

//mxArray *CreateLogical(const mxArray	*A,
//					   const mxArray	*B,
//					   mxClassID		classC) {
//
//	// declare variables
//
//	int			idim;						// index of array dimension
//	int			ndimsA, ndimsB, ndimsC;		// number of array dimensions
//	const int	*sizeA, *sizeB, *sizeC;		// array dimensions
//	mxArray		*C;							// pointer to output array
//
//	// obtain number of array dimensions
//
//    ndimsA = mxGetNumberOfDimensions(A);
//    ndimsB = mxGetNumberOfDimensions(B);
//
//	// obtain array dimensions
//
//    sizeA = mxGetDimensions(A);
//    sizeB = mxGetDimensions(B);
//
//	// if A is scalar, output has size of B
//
//	if (mxGetNumberOfElements(A) == 1) {
//		ndimsC = ndimsB;
//		sizeC = sizeB;
//	}
//
//	// if B is scalar, output has size of A
//
//
//	else if (mxGetNumberOfElements(B) == 1) {
//		ndimsC = ndimsA;
//		sizeC = sizeA;
//	}
//
//	// if neither are scalar, check if input arrays are same size
//
//
//	else {
//        if (ndimsA != ndimsB) {
//            mexErrMsgTxt("Number of array dimensions must match");
//        }
//        for (idim = 0; idim < ndimsA; idim++) {
//            if (sizeA[idim] != sizeB[idim]) {
//                mexErrMsgTxt("Array dimensions must match");
//            }
//        }
//        ndimsC = ndimsA;
//        sizeC = sizeA;
//	}
//
//	// create logical output array
//	// storage is specified by classC
//
//    C = mxCreateNumericArray(ndimsC, sizeC, classC, mxREAL);
//    mxSetLogical(C);
//	return(C);
//}


// CREATENUMERIC - create numeric output array

mxArray *CreateNumeric(const mxArray	*A,
					   const mxArray	*B,
					   mxClassID		classC) {

	// declare variables

	int				idim;						// index of array dimension
	int				ndimsA, ndimsB, ndimsC;		// number of array dimensions
	const int		*sizeA, *sizeB, *sizeC;		// array dimensions
	mxArray			*C;							// pointer to output array
	mxComplexity	complexityC;				// real/complex output

	// obtain number of array dimensions

    ndimsA = mxGetNumberOfDimensions(A);
    ndimsB = mxGetNumberOfDimensions(B);

	// obtain array dimensions

    sizeA = mxGetDimensions(A);
    sizeB = mxGetDimensions(B);

	// if A is scalar, output has size of B

	if (mxGetNumberOfElements(A) == 1) {
		ndimsC = ndimsB;
		sizeC = sizeB;
	}

	// if B is scalar, output has size of A


	else if (mxGetNumberOfElements(B) == 1) {
		ndimsC = ndimsA;
		sizeC = sizeA;
	}

	// if neither are scalar, check if input arrays are same size

	else {
        if (ndimsA != ndimsB) {
            mexErrMsgTxt("Number of array dimensions must match");
        }
        for (idim = 0; idim < ndimsA; idim++) {
            if (sizeA[idim] != sizeB[idim]) {
                mexErrMsgTxt("Array dimensions must match");
            }
        }
        ndimsC = ndimsA;
        sizeC = sizeA;
	}

	// indicate real/complex output

    if (mxIsComplex(A) || mxIsComplex(B)) {
		complexityC = mxCOMPLEX;
	}
	else {
		complexityC = mxREAL;
	}

	// create numeric output array
	// storage is specified by classC

    C = mxCreateNumericArray(ndimsC, sizeC, classC, complexityC);
	return(C);
}


// GETCOMPLEX - recast complex array element value as DOUBLE

complex GetComplex(const mxArray	*input,
				   bool				isComplex,
				   mxClassID		classID,
				   int				index) {

	// declare variables

	complex		result;

	// assign value based on type

	switch (classID) {

	case mxUINT8_CLASS:		// unsigned char
		REAL(result) = (double) *(((uint8_T *) mxGetData(input)) + index);
		if (isComplex) {
			IMAG(result) = (double) *(((uint8_T *) mxGetImagData(input)) + index);
		}
		else {
			IMAG(result) = 0.0;
		}
		break;

	case mxINT8_CLASS:		// signed char
		REAL(result) = (double) *(((int8_T *) mxGetData(input)) + index);
		if (isComplex) {
			IMAG(result) = (double) *(((int8_T *) mxGetImagData(input)) + index);
		}
		else {
			IMAG(result) = 0.0;
		}
		break;

	case mxUINT16_CLASS:	// unsigned short
	case mxCHAR_CLASS:		// MATLAB character (also unsigned short)
		REAL(result) = (double) *(((uint16_T *) mxGetData(input)) + index);
		if (isComplex) {
			IMAG(result) = (double) *(((uint16_T *) mxGetImagData(input)) + index);
		}
		else {
			IMAG(result) = 0.0;
		}
		break;

	case mxINT16_CLASS:		// signed short
		REAL(result) = (double) *(((int16_T *) mxGetData(input)) + index);
		if (isComplex) {
			IMAG(result) = (double) *(((int16_T *) mxGetImagData(input)) + index);
		}
		else {
			IMAG(result) = 0.0;
		}
		break;

	case mxUINT32_CLASS:	// unsigned int
		REAL(result) = (double) *(((uint32_T *) mxGetData(input)) + index);
		if (isComplex) {
			IMAG(result) = (double) *(((uint32_T *) mxGetImagData(input)) + index);
		}
		else {
			IMAG(result) = 0.0;
		}
		break;

	case mxINT32_CLASS:		// signed int
		REAL(result) = (double) *(((int32_T *) mxGetData(input)) + index);
		if (isComplex) {
			IMAG(result) = (double) *(((int32_T *) mxGetImagData(input)) + index);
		}
		else {
			IMAG(result) = 0.0;
		}
		break;

	case mxSINGLE_CLASS:	// single precision
		REAL(result) = (double) *(((real32_T *) mxGetData(input)) + index);
		if (isComplex) {
			IMAG(result) = (double) *(((real32_T *) mxGetImagData(input)) + index);
		}
		else {
			IMAG(result) = 0.0;
		}
		break;

	case mxDOUBLE_CLASS:	// double precision
		REAL(result) = *(mxGetPr(input) + index);
		if (isComplex) {
			IMAG(result) = *(mxGetPi(input) + index);
		}
		else {
			IMAG(result) = 0.0;
		}
		break;

	default:				// invalid type
		mexErrMsgTxt("Invalid numeric type");
		break;
	}

	return(result);
}


// GETDOUBLE - recast real array element value as DOUBLE

double GetDouble(const mxArray	*input,
				 mxClassID		classID,
				 int			index) {

	// declare variables

	double		result;

	// assign value based on type

	switch (classID) {

	case mxUINT8_CLASS:		// unsigned char
		result = (double) *(((uint8_T *) mxGetData(input)) + index);
		break;

	case mxINT8_CLASS:		// signed char
		result = (double) *(((int8_T *) mxGetData(input)) + index);
		break;

	case mxUINT16_CLASS:	// unsigned short
	case mxCHAR_CLASS:		// MATLAB character (also unsigned short)
		result = (double) *(((uint16_T *) mxGetData(input)) + index);
		break;

	case mxINT16_CLASS:		// signed short
		result = (double) *(((int16_T *) mxGetData(input)) + index);
		break;

	case mxUINT32_CLASS:	// unsigned int
		result = (double) *(((uint32_T *) mxGetData(input)) + index);
		break;

	case mxINT32_CLASS:		// signed int
		result = (double) *(((int32_T *) mxGetData(input)) + index);
		break;

	case mxSINGLE_CLASS:	// single precision
		result = (double) *(((real32_T *) mxGetData(input)) + index);
		break;

	case mxDOUBLE_CLASS:	// double precision
		result = *(mxGetPr(input) + index);
		break;

	default:				// invalid type
		mexErrMsgTxt("Invalid numeric type");
		break;
	}

	return(result);
}


// GETFIRSTNSDIM - obtain first non-singleton dimension

int GetFirstNSDim(const mxArray	*prhs[],
				  int			nrhs,
				  int			irhs,
				  const int		*sizeX,
				  int			ndimsX) {

	// declare variables

	int			nsdim;		// first non-singleton dimension
	mxArray		*D;			// command line dimension input
	int			i;			// generic loop counter

	// if input present, check for valid DIM

	if (nrhs > irhs) {
		D = (mxArray *) prhs[irhs];
		if (mxGetNumberOfElements(D) == 1) {
			nsdim = (int) mxGetScalar(D) - 1;
			if ((nsdim < 0) || (nsdim >= ndimsX)) {
				mexErrMsgTxt("DIM exceeds array dimensions");
			}
		}
		else {
			mexErrMsgTxt("DIM must be scalar");
		}
	}

	// otherwise assign DIM to be first non-singleton dimension

	else {
		nsdim = 0;
		for (i = 0; i < ndimsX; i++) {
			if (sizeX[i] > 1) {
				nsdim = i;
				break;
			}
		}
	}
	return(nsdim);
}


// ISFIRSTNSDIM - indicate first non-singleton dimension

bool IsFirstNSDim(const int	*sizeX,
				  int		dimX) {

	bool	isfirst;	// first non-singleton dimension flag
	int		i;			// generic loop counter

	isfirst = true;
	for (i = 0; i < dimX; i++) {
		if (sizeX[i] > 1) {
			isfirst = false;
			break;
		}
	}
	return(isfirst);
}


// VALIDATEINPUTS - check for valid syntax

void ValidateInputs(int				nlhs,
					int				nrhs,
					const mxArray	*prhs[],
					int				minrhs,
					int				maxrhs,
					int				maxlhs) {

	// declare variables

	int		irhs;	// index for rhs pointer array

	// check number of input arguments

	if (nrhs < minrhs) {
        mexErrMsgTxt("Not enough operands");
    }
	else if (nrhs > maxrhs) {
		mexErrMsgTxt("Too many operands");
	}

	// check number of output arguments

	if (nlhs > maxlhs) {
		mexErrMsgTxt("Too many output arguments");
	}
		
	// loop through inputs to check type
	
	for (irhs = 0; irhs < nrhs; irhs++) {

		// arguments must be numeric or character
		
		if (!mxIsNumeric(prhs[irhs]) && !mxIsChar(prhs[irhs])) {
			mexErrMsgTxt("Operands must be numeric or character");
		}

		// cannot process sparse arrays

        if (mxIsSparse(prhs[irhs])) {
            mexErrMsgTxt("Operands cannot be sparse");
        }
	}
}
